#include <stdio.h>
#include "SE3Math.h"
#include "robot.h"
#include "robotdriver.h"
#include "trajPlanner.h"
#include <math.h>

#ifndef PI
#define PI 3.1415926
#endif

int main(){
    Joints joints;
    
    joints.theta1 = 0.0;
    joints.theta2 = -PI/2;
    joints.theta3 = 0.0;
    joints.theta4 = 0.0;
    joints.theta5 = 0.0;
    joints.theta6 = 0.0;

    Joints joints1;
    joints1.theta1 = PI/3;
    joints1.theta2 = PI/4;
    joints1.theta3 = PI/5;
    joints1.theta4 = PI/6;
    joints1.theta5 = PI/7;
    joints1.theta6 = PI/8;

    move_to_home(&joints);
    moveJ(&joints1);

    // Pose pose;
    // forward_kinematics(&joints, &robot, &pose);
    // printf("pose.x = %f \n", pose.x);
    // printf("pose.y = %f \n", pose.y);
    // printf("pose.z = %f \n", pose.z);
    // printf("pose.qx = %f \n", pose.qx);
    // printf("pose.qy = %f \n", pose.qy);
    // printf("pose.qz = %f \n", pose.qz);
    // printf("pose.qw = %f \n", pose.qw);
    // PossibleIK possible_joints;
    // inverse_kinematics(&pose, &robot, &possible_joints);
    // print_joint(possible_joints.ik_results[0]);
    // print_joint(possible_joints.ik_results[1]);
    // print_joint(possible_joints.ik_results[2]);
    // print_joint(possible_joints.ik_results[3]);
    // print_joint(possible_joints.ik_results[4]);
    // print_joint(possible_joints.ik_results[5]);
    // print_joint(possible_joints.ik_results[6]);
    // print_joint(possible_joints.ik_results[7]);
    // Joints best_ik_joints;
    // Joints start_joints;
    // start_joints.theta1 = 0.0;
    // start_joints.theta2 = 0.0;
    // start_joints.theta3 = 0.0;
    // start_joints.theta4 = 0.0;
    // start_joints.theta5 = 0.0;
    // start_joints.theta6 = 0.0;
    // int ik_results = compute_best_ik(&possible_joints, &start_joints, &best_ik_joints);
    // if(ik_results!=0){
    //     print_joint(best_ik_joints);
    // }

    return 0;
}
